import robo3pi as s
import serial
import time
import math
import functions as f


#Define Params
COM_NUM = '4'
ID = '2'
edgeLength = 0.5
timePerEdge = 3


s.init('COM' + COM_NUM) # this is the com the xbee dongle identified itself on my computer.
#f.moveInLine (ID, dist, seconds)
#f.rotateByAngle(ID, 90)


f.moveInLine(ID, 1.2, 1.2*timePerEdge) # Move to point 2
time.sleep(0.5)
f.rotateByAngle(ID, 158) # Rotate towards point 3
time.sleep(0.5)
f.moveInLine(ID,1.08, 1.08*timePerEdge) # Move towards point 3
time.sleep(0.5)
f.rotateByAngle(ID, 235) # Rotate at point 3 towards point 4
time.sleep(0.5)
f.moveInLine(ID, 0.72, 0.72*timePerEdge) # Move to point 4
time.sleep(0.25)
f.rotateByAngle(ID,270) # Rotate at point 4 towards point 5
time.sleep(0.25)
f.moveInLine(ID, 0.72, 0.72*timePerEdge) # Move to point 5
time.sleep(0.25)
f.rotateByAngle(ID, 199) # Rotate at point 5 towards point 6
time.sleep(0.25)
f.moveInLine(ID, 1.28, 1.28*timePerEdge) # Move towards point 6
time.sleep(0.25)


s.close()